#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <vector>
#include <time.h>

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <queue>
#include <omp.h>

#include <jni.h>
#include <android/log.h>


#define  LOG_TAG    "OCV:libnative_activity"
#define  LOGD(...)  __android_log_print(ANDROID_LOG_DEBUG,LOG_TAG,__VA_ARGS__)
#define  LOGI(...)  __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__)
#define  LOGW(...)  __android_log_print(ANDROID_LOG_WARN,LOG_TAG,__VA_ARGS__)
#define  LOGE(...)  __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)


using namespace std;
using namespace cv;

extern "C" {
JNIEXPORT jfloat JNICALL Java_com_example_video_VideoFragment_FindFeatures(JNIEnv *env, jobject obj, jlong addrin, jlong addrout,int addwidth, int addheight,jintArray jtableau,jintArray jSettings);


JNIEXPORT jfloat JNICALL Java_com_example_video_VideoFragment_FindFeatures(JNIEnv *env, jobject obj, jlong addrin, jlong addrout,int addwidth, int addheight,jintArray jtableau,jintArray jSettings)
{
//Initialisation, récuperation des parametre Java
    Mat& mRgba = *(Mat*)addrin;
    Mat& mOutput = *(Mat*)addrout;
    int mWidth= addwidth;
    int mHeight= addheight;
    int mResolution=mWidth*mHeight;
    float ratiopos,ratiorad,pos0,pos1;

    jint* tableau=env->GetIntArrayElements(jtableau,0);	//Recuperation des tableau Java

    jint* Settings=env->GetIntArrayElements(jSettings,0);
    jfloat ratio;
    vector<Vec3f> pfinal;
    Mat mRgbasplit [2];								//Split pour le multithreading
    Scalar hsv_min = Scalar(Settings[2], 50, 50);	//Configuration de la detection des couleurs
    Scalar hsv_max = Scalar(Settings[4], 255, 255);
    Scalar hsv_min2 = Scalar(Settings[3], 50, 50);
    Scalar hsv_max2 = Scalar(Settings[5], 255, 255);

//Super switch
switch(Settings[10])	//Cette partie du tableau correspond à la selection du traitement
{

case 3:{		//Sans traitement, on renvoie juste l'image couleur dans l'output et on libére les tableaux.
	mRgba.copyTo(mOutput);
	 env->ReleaseIntArrayElements(jtableau, tableau, 0);
	 env->ReleaseIntArrayElements(jSettings, Settings, 0);
	 return 0;
	}
break;

case 2:{		//Traitement partiel, sans multithreading pour eviter les problèmes de fusion des images
	//Initialisation
	Mat array255(mHeight,mWidth,CV_8UC1);
	array255.setTo(Scalar(255));
   vector<Point2f>centerfinal;
   vector<float>radiusfinal;
   float ptmpRad;
   Point2f ptmpCent;
//Copie de l'image couleur dans l'output
   mRgba.copyTo(mOutput);
//Preparation des matrices intermédiaires
		Mat threshold_output;
		vector<Mat > contours;
		vector<Vec4i> hierarchy;
		Mat mHSV;
		Mat distance;
		Mat mThresholded;
		Mat mThresholded2;
		Mat mThresholdedend;
		vector<Vec3f> circles;
		vector<Mat> lhsv;
		cvtColor(mRgba, mHSV, COLOR_RGB2HSV);	//Transformation de l'image couleur en mode HSV
		inRange(mHSV, hsv_min, hsv_max, mThresholded);		//filtrage du HSV, premier passage
		inRange(mHSV, hsv_min2, hsv_max2, mThresholded2);	//filtrage du HSV, second passage
		bitwise_or(mThresholded, mThresholded2, mThresholded);	//fusion des images.
		split(mHSV, lhsv);						//Séparation des composantes pour récuperer uniquement la partie S et V.
		Mat S = lhsv[1];
		Mat V = lhsv[2];
		subtract(array255, S, S);				//Soustration des matrices
		subtract(array255, V, V);
		S.convertTo(S, CV_32F);
		V.convertTo(V, CV_32F);
		magnitude(S, V, distance);
	    inRange(distance,Scalar(0),Scalar(200), mThresholded2);
		bitwise_and(mThresholded, mThresholded2, mThresholded);	//Second filtre des couleurs, avec l'aide de S et V et de la magnitude.
		if(Settings[8]>0)
		{erode(mThresholded,mThresholded,Mat(),Point(-1,-1),Settings[9],1,1);}		//Application de l'érosion?
		if(Settings[6]>0)
		{dilate(mThresholded,mThresholded,Mat(),Point(-1,-1),Settings[7],1,1);}		//Application de la dilation.
		mThresholded.copyTo(mThresholdedend);										//Preparation de l'optimisation grand angle

		if(Settings[11]>0)
			{	//Optimisation grand angle
			for (int x = 0; x < mWidth; x++) {
				for (int y = 0; y < mHeight; y++)
				{
					//if((tableau[y*mWidth+x]<mWidth)&&(mThresholded.at<uchar>(y,x)>1))
					mThresholdedend.at<uchar>(y,x)=	mThresholded.at<uchar>(tableau[y*mWidth+x+mResolution],tableau[y*mWidth+x]);
				}
			}
			}

		mThresholdedend.copyTo(mOutput);	//Copie en sortie
	    env->ReleaseIntArrayElements(jtableau, tableau, 0);
	    env->ReleaseIntArrayElements(jSettings, Settings, 0);
	    return 0;
	}
break;


case 1:
	{	//Initialisation du traitment
			Mat array255(mHeight,mWidth/2,CV_8UC1);
			array255.setTo(Scalar(255));
			mRgbasplit[0] = mRgba(Rect(0,0,mWidth/2,mHeight));	//Split de l'input dans deux matrices, séparation de l'image en deux
			mRgbasplit[1] = mRgba(Rect(mWidth/2,0,mWidth/2,mHeight));
		   vector<Point2f>centerfinal;
		   vector<float>radiusfinal;
		   float ptmpRad;
		   Point2f ptmpCent;

		   mRgba.copyTo(mOutput);
		#pragma omp parallel for shared(centerfinal,radiusfinal)		//Multithreading, séparation en deux processus
		for(int p=0 ; p<2 ; p++)
			{
				Mat threshold_output;
				vector<Mat > contours;
				vector<Vec4i> hierarchy;
				Mat mHSV;
				Mat distance;
				Mat mThresholded;
				Mat mThresholded2;
				Mat mThresholdedend;
				vector<Vec3f> circles;
				vector<Mat> lhsv;
				cvtColor(mRgbasplit[p], mHSV, COLOR_RGB2HSV);		//HSV
				inRange(mHSV, hsv_min, hsv_max, mThresholded);
				inRange(mHSV, hsv_min2, hsv_max2, mThresholded2);
				bitwise_or(mThresholded, mThresholded2, mThresholded);		//Filtre de la couleur (Premier passage)
				split(mHSV, lhsv);
				Mat S = lhsv[1];
				Mat V = lhsv[2];
				subtract(array255, S, S);
				subtract(array255, V, V);
				S.convertTo(S, CV_32F);
				V.convertTo(V, CV_32F);
				magnitude(S, V, distance);
			    inRange(distance,Scalar(0),Scalar(200), mThresholded2);
				bitwise_and(mThresholded, mThresholded2, mThresholded);		//Filtre de la couleur (Second passage)
				if(Settings[8]>0)
				{erode(mThresholded,mThresholded,Mat(),Point(-1,-1),Settings[9],1,1);}		//Erosion
				if(Settings[6]>0)
				{dilate(mThresholded,mThresholded,Mat(),Point(-1,-1),Settings[7],1,1);}		//Dilatation
				mThresholded.copyTo(mThresholdedend);
				if(Settings[11]>0)		//Optimisation grand angle.
					{
					if(p==0)
				{
					for (int x = 0; x < mWidth/2; x++) {
						for (int y = 0; y < mHeight; y++)
						{
							if((tableau[y*mWidth+x]<mWidth/2)&&(mThresholded.at<uchar>(y,x)>1))
							mThresholdedend.at<uchar>(y,x)=	mThresholded.at<uchar>(tableau[y*mWidth+x+mResolution],tableau[y*mWidth+x]);
						}
					}
				}
				else
				{
					for (int x = mWidth/2; x < mWidth; x++) {
						for (int y = 0; y < mHeight; y++)
						{
						//	mThresholdedend.at(x,y) = mThresholded[tableau[y*mWidth+x+mWidth*mHeight] * mWidth + tableau[y*mWidth+x]];
							if((tableau[y*mWidth+x]<mWidth/2)&&(mThresholded.at<uchar>(y,x)>1))
							mThresholdedend.at<uchar>(y,x) = mThresholded.at<uchar>(tableau[y*mWidth+x+mResolution],tableau[y*mWidth+x]);
						}
					}

				}
					}

				findContours( mThresholdedend, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );	//Detection des contours

				 if(!contours.empty())
				 {
					 vector < Mat > contours_poly( contours.size() );
					 vector<Point2f>center( contours.size() );
					 vector<float>radius( contours.size() );

				   for( size_t i = 0; i < contours.size(); i++ )
				   {

					   approxPolyDP( contours[i], contours_poly[i], 3, true );		//Detection de polynomes.

					  // if((contours_poly[i].total()>5))
					   //{
				        minEnclosingCircle( contours_poly[i], center[i], radius[i] );	//Cette fonction entoure tous les polynomes autours de cercles.
				        if(p==1)
				        	{
				        	center[i].x+=mWidth/2;	//Ajout de la moitié de la largeur pour tous les cercles de la seconde partie de l'image (Multithreading).
				        	}
				        if(radius[i]>Settings[13])	//Si le diametre minimal de l'image est inferieur à celui configuré, on ne le rajoute pas dans la matrice finale
				        {
				        centerfinal.push_back(center[i]);
				        radiusfinal.push_back(radius[i]);
				        }
					 //  }
				   }




				 }
			}
//On quitte le multithreading et on traite
		  env->ReleaseIntArrayElements(jtableau, tableau, 0);	//Liberation du tableau des grand angle.

			 if(!radiusfinal.empty())
		  {
			for( size_t j = 0; j < radiusfinal.size(); j++ )	//On tri la taille des cercles de façon décroissante
			{
			 for( size_t i = 1; i < radiusfinal.size(); i++ )
			 {
				 if(radiusfinal[i-1]<radiusfinal[i])
				 {
					ptmpRad=radiusfinal[i-1];
					radiusfinal[i-1]=radiusfinal[i];
					radiusfinal[i]=ptmpRad;
					ptmpCent=centerfinal[i-1];
					centerfinal[i-1]=centerfinal[i];
					centerfinal[i]=ptmpCent;
				  }
			  }
			}
		  }
	if((centerfinal.size()>=2))	//On vérifie que le tableau posséde au moins deux cercles et on prend uniquement les deux premiers.
		{

		if((radiusfinal[1]>radiusfinal[0]-radiusfinal[1]/4)&&(radiusfinal[1]<radiusfinal[0]+radiusfinal[1]/4))	//On fait en sorte que les cercles soient approximativement de même taille.
		{

		}
		else if (centerfinal.size()>=3)
		{
		 if ((radiusfinal[1]>radiusfinal[2]-radiusfinal[1]/4)&&(radiusfinal[1]<radiusfinal[2]+radiusfinal[1]/4))	//S'il y a une grosse différence de taille, on test la balise suivante (Si elle existe)...Si il y a toujours un problème sur la taille, on renvoie un 404
		 {
				ptmpCent=centerfinal[1];
				centerfinal[1]=centerfinal[2];
				centerfinal[0]=ptmpCent;
				ptmpRad=radiusfinal[1];
				radiusfinal[1]=radiusfinal[2];
				radiusfinal[0]=ptmpRad;
		 }
		 else
		 {
			return 404;
		 }
		}
		else
		{
			return 404;
		}
				if(centerfinal[1].x-centerfinal[0].x<0)		//Celui de gauche est placé en "0", celui de droite en "1"
				{
					ptmpCent=centerfinal[1];
					centerfinal[1]=centerfinal[0];
					centerfinal[0]=ptmpCent;
					ptmpRad=radiusfinal[1];
					radiusfinal[1]=radiusfinal[0];
					radiusfinal[0]=ptmpRad;
				}

				if(abs(centerfinal[0].x-centerfinal[1].x)>30)  //Distance minimum à detecter entre deux balises.
				{
				if(((centerfinal[0].x<mWidth/2)&&(centerfinal[1].x>mWidth/2)))	//Detecte que les deux plus grosses balises sont bien sur les deux cotés de l'ecran.
				{

					//CALCUL RATIO EN FONCTION DU RADIAN

				if(radiusfinal[0] > radiusfinal[1])
				{
					ratiorad = (radiusfinal[0] - radiusfinal[1]) / radiusfinal[0];
				}
				else if (radiusfinal[0] < radiusfinal[1])
				{
					ratiorad = (radiusfinal[0] - radiusfinal[1]) / radiusfinal[1];
				}
				else
				{
					ratiorad = 0;
				}
					//CALCUL RATIO EN FONCTION DE LA POSITION

				pos0=abs(mWidth/2-centerfinal[0].x);
				pos1=abs(centerfinal[1].x-mWidth/2);

				ratiopos=(pos0-pos1)/(mWidth/Settings[14]);

				ratio=ratiopos;

				env->ReleaseIntArrayElements(jSettings, Settings, 0);		//Liberation du tableau des settings

				 for( size_t i = 0; i< 2; i++ )
				  {
					 Scalar color = Scalar( 255,255,0 );
					 circle( mOutput, centerfinal[i], (int)radiusfinal[i], color, 2, 8, 0 );	//On trace les cercles detecté sur l'image.
				  }

				return ratio;			//On renvoie le ratio

				}
				}




	}
	}
break;

default: break;
}
env->ReleaseIntArrayElements(jSettings, Settings, 0);
return 404;
}

}
